/*
 * Accelerometer.cpp
 *
 *  Created on: Jan 14, 2014
 *      Author: ppks
 */

#include "common.h"
#include "IMU_Common.h"
#include "i2c.h"
#include "FlightController_Common.h"
#include "EEPROM.h"

/******************************************************************************/
/* Macro Definition                                                           */
/******************************************************************************/
#if !defined(RECORD_ACC_ORIENTATION_DATA)
  #define RECORD_ACC_ORIENTATION_DATA(X, Y, Z)  {imuData.accADC[ROLL]  = -X; imuData.accADC[PITCH]  = -Y; imuData.accADC[YAW]  = Z;}
#endif

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/
static void Accelerometer_Common(void);

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
/*******************************************************************************
* Function Name: Accelerometer_Init()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void Accelerometer_Init(void)
{
#if (ACCELEROMETER_TYPE == MPU6050)
    CommI2C_WriteReg(ACCL_ADDRESS, 0x1C, 0x10);             /* ACCEL_CONFIG  -- AFS_SEL=2 (Full Scale = +/-8G)  ; ACCELL_HPF=0   (note something is wrong in the spec.) */
    /* note: something seems to be wrong in the spec here. With AFS=2 1G = 4096 but according to my measurement: 1G=2048 (and 2048/8 = 256) */
    /* confirmed here: http://www.multiwii.com/forum/viewtopic.php?f=8&t=1080&start=10#p7480 */

#if defined(MPU6050_I2C_AUX_MASTER)
    /* at this stage, the MAG is configured via the original MAG init function in I2C bypass mode */
    /* now we configure MPU as a I2C Master device to handle the MAG via the I2C AUX port (done here for HMC5883) */
    CommI2C_WriteReg(ACCL_ADDRESS, 0x6A, 0b00100000);       //USER_CTRL     -- DMP_EN=0 ; FIFO_EN=0 ; I2C_MST_EN=1 (I2C master mode) ; I2C_IF_DIS=0 ; FIFO_RESET=0 ; I2C_MST_RESET=0 ; SIG_COND_RESET=0
    CommI2C_WriteReg(ACCL_ADDRESS, 0x37, 0x00);             //INT_PIN_CFG   -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
    CommI2C_WriteReg(ACCL_ADDRESS, 0x24, 0x0D);             //I2C_MST_CTRL  -- MULT_MST_EN=0 ; WAIT_FOR_ES=0 ; SLV_3_FIFO_EN=0 ; I2C_MST_P_NSR=0 ; I2C_MST_CLK=13 (I2C slave speed bus = 400kHz)
    CommI2C_WriteReg(ACCL_ADDRESS, 0x25, 0x80|MAG_ADDRESS); //I2C_SLV0_ADDR -- I2C_SLV4_RW=1 (read operation) ; I2C_SLV4_ADDR=MAG_ADDRESS
    CommI2C_WriteReg(ACCL_ADDRESS, 0x26, MAG_DATA_REGISTER);//I2C_SLV0_REG  -- 6 data bytes of MAG are stored in 6 registers. First register address is MAG_DATA_REGISTER
    CommI2C_WriteReg(ACCL_ADDRESS, 0x27, 0x86);             //I2C_SLV0_CTRL -- I2C_SLV0_EN=1 ; I2C_SLV0_BYTE_SW=0 ; I2C_SLV0_REG_DIS=0 ; I2C_SLV0_GRP=0 ; I2C_SLV0_LEN=3 (3x2 bytes)
#endif
#endif /* (ACCELEROMETER_TYPE == MPU6050) */
}

/*******************************************************************************
* Function Name: Accelerometer_ReadRawADC()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void Accelerometer_ReadRawADC(void)
{
    uint8_t rawADC[6];

    /* Read the data from accelerometer */
#if (ACCELEROMETER_TYPE == MPU6050)
    CommI2C_ReadRegToBuffer(ACCL_ADDRESS, ACCL_DATA_REGISTER, rawADC, 6);

    /* Record the data into the global structure */
    RECORD_ACC_ORIENTATION_DATA( ((rawADC[0]<<8) | rawADC[1])>>3 ,  /* X */
                                 ((rawADC[2]<<8) | rawADC[3])>>3 ,  /* Y */
                                 ((rawADC[4]<<8) | rawADC[5])>>3 ); /* Z */
#endif /* (ACCELEROMETER_TYPE == MPU6050) */

    /* Execute common routine to apply calibration data or perform calibration */
    Accelerometer_Common();
}

/*******************************************************************************
* Function Name: Accelerometer_ReadRawADC()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void Accelerometer_Common(void)
{
    static int32_t a[3];

    /* Check if calibration is required */
    if (calibration.AccCycles > 0)
    {
        for (uint8_t axis = 0; axis < 3; axis++)
        {
            /* Reset a[axis] at start of calibration */
            if (calibration.AccCycles == 512)
            {
                a[axis] = 0;
            }

            /* Sum up 512 readings */
            a[axis] += imuData.accADC[axis];

            /* Clear global variables for next reading */
            imuData.accADC[axis] = 0;
            calData.accBaseline[axis] = 0;
        }

        /* Calculate average, shift Z down by ACC_1G and store values in EEPROM at end of calibration */
        if (calibration.AccCycles == 1)
        {
            calData.accBaseline[ROLL]  = (a[ROLL] + 256) >> 9;
            calData.accBaseline[PITCH] = (a[PITCH] + 256) >> 9;
            calData.accBaseline[YAW]   = ((a[YAW] + 256) >> 9) - ACC_1G_VALUE;
            FC_Config.RC.angleTrim[ROLL]   = 0;
            FC_Config.RC.angleTrim[PITCH]  = 0;

            /* Set a flag that calibration completed */
            SetAccCalibratedFlag();

#if UPDATE_CAL_DATA_AFTER_ANY_SENSOR
            /* Update calibration data in EEPROM */
            EEPROM_UpdateCalibrationData();
#endif /* UPDATE_CAL_DATA_AFTER_ANY_SENSOR */
        }
        calibration.AccCycles--;
    }

    /* Subtract calibration values */
    imuData.accADC[ROLL]  -=  calData.accBaseline[ROLL] ;
    imuData.accADC[PITCH] -=  calData.accBaseline[PITCH];
    imuData.accADC[YAW]   -=  calData.accBaseline[YAW] ;

#if defined(SENSORS_TILT_45DEG_LEFT)
    int16_t temp = ((imuData.accADC[PITCH] - imuData.accADC[ROLL] ) * 7) / 10;
    imuData.accADC[ROLL] = ((imuData.accADC[ROLL]  + imuData.accADC[PITCH]) * 7) / 10;
    imuData.accADC[PITCH] = temp;
#endif /* defined(SENSORS_TILT_45DEG_LEFT) */
#if defined(SENSORS_TILT_45DEG_RIGHT)
    int16_t temp = ((imuData.accADC[PITCH] + imuData.accADC[ROLL] ) * 7) / 10;
    imuData.accADC[ROLL] = ((imuData.accADC[ROLL]  - imuData.accADC[PITCH]) * 7) / 10;
    imuData.accADC[PITCH] = temp;
#endif /* defined(SENSORS_TILT_45DEG_LEFT) */
}


